//
// Created by oftenlin on 23-9-11.
//
#include "NaviData.h"
#include "simpleRtk.hpp"
namespace NaviData {
    const double THRESHLOD_ = 1e-6;

//    const int MAXSECONDOFDAY = 86400;
//    const int MAXSECONDOFWEEK = 86400 * 7;

    NavTime::NavTime(const gtime_t &init_gpstime) : time_type_(NaviData::TIMETYPE::GPSTIME), mcu_time_(-1.0) {
        this->gpstime_.gtime = init_gpstime;
        this->gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
        this->mcu_time_ = 0.0;
    }

    NavTime::NavTime(const double mcu_time_) : time_type_(NaviData::TIMETYPE::MCUTIME), gpstime_{0, 0.0},
                                               mcu_time_(mcu_time_) {
        this->gpstime_.week = 0;
        this->gpstime_.tow = 0.0;
    }

    NavTime::NavTime(const NavTime &time_) {
        this->time_type_ = time_.time_type_;
        this->gpstime_.gtime = time_.gpstime_.gtime;
        this->mcu_time_ = time_.mcu_time_;
        this->gpstime_.week = time_.gpstime_.week;
        this->gpstime_.tow = time_.gpstime_.tow;
    }
// 使用周秒数初始化
//    NavTime::NavTime(int week_, double tow_){
//        this->time_type_ = TIMETYPE::GPSTIME;
//        this->gpstime_.gtime = simpleRtk::gpst2time(week_, tow_);
//        this->gpstime_.week = week_;
//        this->gpstime_.tow = tow_;
//        this->mcu_time_ = 0.0;
//    }

/**
 * @brief  operator + for NavTime and seconds
 * @note
 * @param  second:
 * @retval
 */
    NavTime NavTime::operator+(double second) {
        NavTime res(*this);
        if (this->time_type_ == TIMETYPE::GPSTIME) {
            res.gpstime_.gtime = simpleRtk::timeadd(this->gpstime_.gtime, second);
            res.gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
            res.mcu_time_ += second;
        }
        return res;
    }

/**
 * @brief  operator - for NavTime and seconds
 * @note
 * @param  second:
 * @retval
 */
    NavTime NavTime::operator-(double second) {
        NavTime res(*this);
        if (this->time_type_ == TIMETYPE::GPSTIME) {
            res.gpstime_.gtime = simpleRtk::timeadd(this->gpstime_.gtime, -second);
            res.gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
            res.mcu_time_ -= second;
        }
        return res;
    }

    NavTime &NavTime::operator=(const NavTime &init_navtime) {
        if (this == &(init_navtime)) return *this;

        this->time_type_ = init_navtime.time_type_;
        this->gpstime_.gtime = init_navtime.gpstime_.gtime;
        this->mcu_time_ = init_navtime.mcu_time_;
        this->gpstime_.week = init_navtime.gpstime_.week;
        this->gpstime_.tow = init_navtime.gpstime_.tow;

        return *this;
    }

    double NavTime::operator-(const NavTime &time) const {
        if (this->time_type_ == TIMETYPE::MCUTIME) {
            return this->mcu_time_ - time.mcu_time_;
        } else if (this->time_type_ == TIMETYPE::GPSTIME) {
            return simpleRtk::timediff(this->gpstime_.gtime, time.gpstime_.gtime);
        } else {
            //VDR_LOGI("time_type_unknown");
            return 0.0;
        }
    }

/**
 * @brief  compare two NavTime with <
 * @note
 * @param  &time:
 * @retval bool, if this < time return true
 */
    bool NavTime::operator<(const NavTime &time) const {
        if (this->time_type_ == TIMETYPE::GPSTIME) {
            double dt = simpleRtk::timediff(this->gpstime_.gtime, time.gpstime_.gtime);
            if (dt < -THRESHLOD_) return true;
            else return false;
        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
            double dt = this->mcu_time_ - time.mcu_time_;
            if (dt < -THRESHLOD_) return true;
            else return false;
        } else {
            return false;
        }
    }

/**
 * @brief  compare two NavTime with >
 * @note
 * @param  &time:
 * @retval bool, if this > time return true
 */
    bool NavTime::operator>(const NavTime &time) const {
        if (this->time_type_ == TIMETYPE::GPSTIME) {
            double dt = simpleRtk::timediff(this->gpstime_.gtime, time.gpstime_.gtime);
            if (dt > THRESHLOD_) return true;
            else return false;
        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
            double dt = this->mcu_time_ - time.mcu_time_;
            if (dt > THRESHLOD_) return true;
            else return false;
        } else {
            return false;
        }
    }

/**
 * @brief  compare two NavTime with >
 * @note
 * @param  &time:
 * @retval bool, if this == time return true
 */
    bool NavTime::operator==(const NavTime &time) const {

        if (this->time_type_ == TIMETYPE::GPSTIME) {
            double dt = simpleRtk::timediff(this->gpstime_.gtime, time.gpstime_.gtime);
            if (fabs(dt) <= THRESHLOD_) return true;
            else return false;
        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
            double dt = this->mcu_time_ - time.mcu_time_;
            if (fabs(dt) <= THRESHLOD_) return true;
            else return false;
        }
        return false;
    }

    void NavTime::setNavtime(double mcu_time) {
        this->time_type_ = TIMETYPE::MCUTIME;
        this->mcu_time_ = mcu_time;
    }

    void NavTime::setNavtime(const gtime_t &gtime) {
        this->time_type_ = TIMETYPE::GPSTIME;
        this->gpstime_.gtime = gtime;
        this->gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
    }

    std::ostream &operator<<(std::ostream &output, const NavTime &nav_time) {
//        if(nav_time.time_type_ == TIMETYPE::GPSTIME_WEEK_TOW){
//            output << fmt::format("{},{},{:.4f}", constant::GPSTIMETAG, nav_time.gpstime_.week, nav_time.gpstime_.tow);
//        }else if(nav_time.time_type_ == TIMETYPE::MCUTIME){
//            output << fmt::format("{},{:.4f}",constant::GPSTIMETAG,nav_time.mcu_time_);
//        }
        return output;
    }

    std::string NavTime::getTimeStr() const {

//        if (this->time_type_ == TIMETYPE::GPSTIME) {
//            std::string timeStr = FMT_STR("GPSTIME,%.5f", this->gpstime_.gtime.time + this->gpstime_.gtime.sec);
//            return timeStr;
//        } else if (this->time_type_ == TIMETYPE::MCUTIME) {
//            std::string timeStr = FMT_STR("mcutime,%.3f", this->mcu_time_);
//            return timeStr;
//        } else {
//            return "";
//        }
        return "";

    }

/***
 * @brief 根据固定步长生成对应的输出时间列表
 * @param t0
 * @param t1
 * @param stepsize
 * @return
 */
    std::vector<NavTime> getFixefStepTime(const NavTime &t0, const NavTime &t1, double stepsize) {

        std::vector<NavTime> res;

        //TODO 增加GPSTIME的拟合
        if (t0.time_type_ == GPSTIME) {

            auto time_diff = simpleRtk::timediff(t1.gpstime_.gtime, t0.gpstime_.gtime);
            int steps = time_diff * 1000 / stepsize + 1;
            gtime_t start_time = t0.gpstime_.gtime;
            start_time.sec = int(start_time.sec * 1000 / stepsize) * stepsize / 1000.0;

            for (int i = 0; i <= steps; ++i) {
                auto tmp = simpleRtk::timeadd(start_time, i * stepsize / 1000.0);
                if (simpleRtk::timediff(tmp, t0.gpstime_.gtime) < 0) continue;
                if (simpleRtk::timediff(tmp, t1.gpstime_.gtime) > 0) break;
                NavTime cur_navtime(tmp);
                res.emplace_back(std::move(cur_navtime));
            }

        } else if (t0.time_type_ == MCUTIME) {

            int base = int(t0.mcu_time_);
            int t0_stepsize = int((t0.mcu_time_ - base) * 1000 / stepsize);
            int t1_stepsize = int((t1.mcu_time_ - base) * 1000 / stepsize) + 1;
            for (int i = 0; i < (t1_stepsize - t0_stepsize); ++i) {
                if (t0_stepsize + i < ((t0.mcu_time_ - base) * 1000 / stepsize)) continue;
                if (t0_stepsize + i > ((t1.mcu_time_ - base) * 1000 / stepsize)) break;
                NavTime tmp(base + (t0_stepsize + i) * stepsize / 1000.0);
                res.emplace_back(std::move(tmp));
            }

        } else {
            //VDR_LOGI("getFixefStepTime,time_type_unkown");
        }
        return res;
    }

    bool NavTime::isZero() {
        if (time_type_ == TIMETYPE::MCUTIME) {
            if (fabs(mcu_time_) < 1e-6) {
                return true;
            }
        } else if (time_type_ == TIMETYPE::GPSTIME) {
            if (fabs(this->gpstime_.gtime.time + this->gpstime_.gtime.sec) < 1e-6) {
                return true;
            }
        } else {
            return false;
        }
        return false;
    }

/*
 * 获取时间戳（s）
 * return -1 invalid
 */
    double NavTime::getTimeStamps() const {
        if (time_type_ == TIMETYPE::MCUTIME) {
            return this->mcu_time_;
        } else if (time_type_ == TIMETYPE::GPSTIME) {
            return this->gpstime_.gtime.time + this->gpstime_.gtime.sec;
        }
        return -1;
    }

    NavTime::NavTime() {
        this->time_type_ = GPSTIME;
        this->mcu_time_ = 0.0;
        this->gpstime_.gtime = gtime_t{0, 0.0};
        this->gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
    }

    const TIMETYPE NavTime::getTimeType() {
        return time_type_;
    }

    void NavTime::setZero() {
        this->time_type_ = GPSTIME;
        this->mcu_time_ = 0.0;
        this->gpstime_.gtime = gtime_t{0, 0.0};
        this->gpstime_.tow = simpleRtk::time2gpst(this->gpstime_.gtime, &(this->gpstime_.week));
    }
    BaseData::BaseData(const NavTime &time, DataType init_data_type) : t0_(time), data_type_(init_data_type) {}

    BaseData::BaseData() : data_type_(DataType::DATAUNKOWN), t0_(NavTime(0)) {}

    NavTime BaseData::get_time() const { return t0_; }

    void BaseData::set_time(const NavTime &time) { t0_ = time; }

    DataType BaseData::get_type() const { return data_type_; }


    GnssData::GnssData(const NavTime &time, GnssData::GNSSOBSDetail &init_gnssobs_detail) : BaseData(time, GNSSDATA),
                                                                                            gnss_obs_detail_(
                                                                                                    init_gnssobs_detail) {}

    GnssData::GnssData() : BaseData(NavTime(0), GNSSDATA) {}

    const GnssData::GNSSOBSDetail &GnssData::getGnssObsDetail() {
        return this->gnss_obs_detail_;
    }

    //    void GnssData::setGnssData(const NavTime &cur_navtime, const GnssData::GNSSOBSDetail &cur_gnssobs_detail) {
    //        GnssData::BaseData::set_time(cur_navtime);
    //        this->gnss_obs_detail_ = cur_gnssobs_detail;
    //    }

    NavTime GnssData::get_time() const {
        return this->BaseData::get_time();
    }

    //    void GnssData::setGnssData(NavTime &&cur_navtime, GnssData::GNSSOBSDetail &&cur_gnssobs_detail) {
    //        GnssData::BaseData::set_time(cur_navtime);
    //        this->gnss_obs_detail_ = cur_gnssobs_detail;
    //    }

    ImuData::ImuDataDetail &ImuData::ImuDataDetail::operator=(const ImuData::ImuDataDetail &input) {
        if (&input != this) {
            gyro_ = input.gyro_;
            acce_ = input.acce_;
            dtheta = input.dtheta;
            dvel = input.dvel;
            dt = input.dt;
        }
        return *this;
    }

    ImuData::ImuData(const ImuData &in_imudata)
            : BaseData(in_imudata.get_time(), IMUDATA), imudata_detail_(in_imudata.imudata_detail_) {}

    ImuData::ImuData(const NavTime &time, const ImuData::ImuDataDetail &init_imudata_detail) : BaseData(time, IMUDATA),
                                                                                               imudata_detail_(
                                                                                                       init_imudata_detail) {}

    ImuData::ImuData() : BaseData(NavTime(0), IMUDATA) {}

    void ImuData::setImuData(const NavTime &cur_navtime, const ImuData::ImuDataDetail &cur_imudata_detail) {
        set_time(cur_navtime);
        imudata_detail_ = cur_imudata_detail;
    }

    NavTime ImuData::get_time() const {
        return this->BaseData::get_time();
    }

    const ImuData::ImuDataDetail &ImuData::getImuDataDetail() { return this->imudata_detail_; }

    /***
     * @brief 对当前的数据进行分割，获取前后两个历元的IMU数据
     * @param cur_navtime 被分割的时间点
     * @return
     */
    ImuData ImuData::splitData(const NavTime &cur_navtime) {
        auto time_ = BaseData::get_time();
        double dt = this->imudata_detail_.dt - (time_ - cur_navtime);

        ImuData imu_data;
        ImuData::ImuDataDetail cur_imu_data_detail = this->imudata_detail_;
        cur_imu_data_detail.dt = dt;
        double rate = cur_imu_data_detail.dt / this->imudata_detail_.dt;
        for (int i = 0; i < 3; ++i) {
            cur_imu_data_detail.dvel[i] = rate * this->imudata_detail_.dvel[i];
            cur_imu_data_detail.dtheta[i] = rate * this->imudata_detail_.dtheta[i];
        }
        imu_data.setImuData(cur_navtime, cur_imu_data_detail);

        // 计算被分割后的数据
        this->imudata_detail_.dt -= dt;
        for (int i = 0; i < 3; ++i) {
            this->imudata_detail_.dvel[i] -= cur_imu_data_detail.dvel[i];
            this->imudata_detail_.dtheta[i] -= cur_imu_data_detail.dtheta[i];
        }

        return imu_data;
    }

    CANVelocity::CANVelocity(const NavTime &time) : BaseData(time, CANVELOCITYDATA) {}

    CANVelocity::CANVelocity() : BaseData(NavTime(0), CANVELOCITYDATA) {}

    NavTime CANVelocity::get_time() const {
        return this->BaseData::get_time();
    }

    void CANVelocity::setCanVelocity(const NavTime &cur_navtime,
                                     const CANVelocity::CANVelocityDetail &cur_canVelocity_detail) {
        CANVelocity::BaseData::set_time(cur_navtime);
        this->CanVelocity_detail = cur_canVelocity_detail;
    }

    const CANVelocity::CANVelocityDetail &CANVelocity::getCanVelocityDetail() {
        return this->CanVelocity_detail;
    }

    CameraData::CameraData(const NavTime &time) : BaseData(time, CAMERADATA) {}

    CameraData::CameraData(const CameraData &camera) : BaseData(camera.get_time(), camera.get_type()) {}

    CameraData &CameraData::operator=(const CameraData &camera) {
        if (this == &camera) return *this;
        this->camera_data_detail_ = camera.camera_data_detail_;
        return *this;
    }

    NavTime CameraData::get_time() const {
        return this->BaseData::get_time();
    }

    NavInfo::NavInfo(const gtime_t &gtime) : time_(gtime) {}

    NavTime LinkData::get_time() const {
        return this->BaseData::get_time();
    }

    //    void LinkData::setLinkData(const NavTime &cur_navtime, const LinkData::LinkDataDetail &cur_link_detail) {
    //        LinkData::BaseData::set_time(cur_navtime);
    //        this->mLinkDetail_ = cur_link_detail;
    //    }

    const LinkData::LinkDataDetail &LinkData::getLinkDataDetail() {
        return this->mLinkDetail_;
    }

    LinkData::LinkData() : BaseData(NavTime(0), SDMMDATA) {}

    //注意输入的ms转s
    LinkData::LinkData(linkInfoGcj02 &curLinkInfo) : BaseData(double(curLinkInfo.timestamp * 1e-3), SDMMDATA) {
        this->mLinkDetail_.pos_[0] = curLinkInfo.plan_proj_lat * D2R;
        this->mLinkDetail_.pos_[1] = curLinkInfo.plan_proj_lon * D2R;
        this->mLinkDetail_.pos_[2] = 0.0;
        this->mLinkDetail_.direction = curLinkInfo.plan_direction;
        this->mLinkDetail_.isTunnel = curLinkInfo.isTunnel;
        this->mLinkDetail_.canUse = curLinkInfo.canUse2Fuse && curLinkInfo.isValid();
    }
}